
#include "Engine.hpp"


Engine::Engine() :
	leftWheel(Wheel(Wheel::LEFT)),
	rightWheel(Wheel(Wheel::RIGHT))
{

}

void Engine::driveForward()
{
	currentState = FORWARD;
	leftWheel.forward();
	rightWheel.forward();
}

void Engine::driveBackward()
{
	currentState = BACKWARD;
	leftWheel.backward();
	rightWheel.backward();
}

void Engine::turnLeft()
{
	currentState = LEFT;
	leftWheel.backward();
	rightWheel.forward();
}

void Engine::turnRight()
{
	currentState = RIGHT;
	leftWheel.forward();
	rightWheel.backward();
}

void Engine::stop()
{
	currentState = STOP;
	leftWheel.stop();
	rightWheel.stop();
}

void Engine::setState(State state)
{
	switch (state)
	{
		case FORWARD:
		{
			driveForward();
			break;
		}
		case BACKWARD:
		{
			driveBackward();
			break;
		}
		case LEFT:
		{
			turnLeft();
			break;
		}
		case RIGHT:
		{
			turnRight();
			break;
		}
		case STOP:
		{
			stop();
			break;
		}
		default:
		{
			break;
		}
	}
}

Engine::State Engine::getState()
{
	return currentState;
}
